#include <iostream>
#include <fstream>
#include <iterator>                            // STL标准中的一些迭代器模板类
#include <string>
#include <vector>
#include <opencv2/opencv.hpp>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>         // 高层GUI图像交互模块
#include <nav_msgs/Odometry.h>                 // ROS导航功能包里的里程计
#include <nav_msgs/Path.h>                     // 用于显示轨迹
#include <ros/ros.h>
#include <rosbag/bag.h>
#include <geometry_msgs/PoseStamped.h>         // 时间、位置和姿态
#include <cv_bridge/cv_bridge.h>               // 在ROS图像消息和OpenCV图像之间进行转换的一个功能包
#include <sensor_msgs/image_encodings.h>       // 关于图像编码模式的源文件，其中规定了RGB的图像以及深度图的编码模式
#include <eigen3/Eigen/Dense>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>   // 一些数据类型转换的函数
#include <sensor_msgs/PointCloud2.h>

std::vector<float> read_lidar_data(const std::string lidar_data_path)
{
    // c++中文件读取和写入以流的形式进行
    // 函数原型 ifstream(const char *filename, std::ifstream::openmode mode);
    // ifstream 打开文件，mode模式std::ifstream::in | std::ifstream::binary：以只读方式打开 | 以二进制而非文本格式进行操作
    std::ifstream lidar_data_file(lidar_data_path, std::ifstream::in | std::ifstream::binary);
    // seekg()用于设置在输入流中的位置，seek + get
    // 新位置将从由 place 给出的起始位置开始，偏移 offset 个字节。offset 形参是一个 long 类型的整数，而 place 可以是以下 ios 类中的之一
    // 起始位置可能是文件的开头、文件的当前位置或文件的末尾，这些地方分别由常量 ios::beg、ios::cur 和 ios::end 表示。
    lidar_data_file.seekg(0, std::ios::end);
    // size_t 一个与机器相关的unsigned类型。统计一下文件有多少float数据
    // tellg() 用于在输入流中获取位置
    const size_t num_elements = lidar_data_file.tellg() / sizeof(float);
    // 文件指针指向文件开始
    lidar_data_file.seekg(0, std::ios::beg);
    // 定义float类型的容器，雷达数据缓冲（数据量）
    std::vector<float> lidar_data_buffer(num_elements);
    // 读取所有的数据
    // .read（）文件读取，有两个参数，从 lidar_data_file读取num_elements*sizeof(float)数量的数据到&lidar_data_buffer[0]里面
    lidar_data_file.read(reinterpret_cast<char *>(&lidar_data_buffer[0]), num_elements * sizeof(float));
    return lidar_data_buffer;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "kitti_helper");
    ros::NodeHandle n("~");
    std::string dataset_folder, sequence_number, output_bag_file;
    // 是否将数据集转换为rosbag文件格式
    bool to_bag;
    // 发布延迟时间
    int publish_delay = 1;
    n.getParam("to_bag", to_bag);
    n.getParam("publish_delay", publish_delay);
    n.getParam("dataset_folder", dataset_folder);
    n.getParam("sequence_number", sequence_number);
    if (to_bag) n.getParam("output_bag_file", output_bag_file);
    std::cout << "Reading sequence " << sequence_number << " from " << dataset_folder << '\n';
    publish_delay = publish_delay <= 0 ? 1 : publish_delay;
    // 创建一个publisher，发布名为/velodyne_points的topic，消息类型为sensor_msgs::PointCloud2，队列长度为2
    ros::Publisher pub_laser_cloud = n.advertise<sensor_msgs::PointCloud2>("velodyne_points", 2);
    // 里程计发布，odomGT，包括旋转的四元数和位置坐标向量
    ros::Publisher pubOdomGT = n.advertise<nav_msgs::Odometry>("odometry_gt", 5);
    // 发布轨迹 pathGT
    ros::Publisher pubPathGT = n.advertise<nav_msgs::Path>("path_gt", 5);
    // image_transport发布图像 提供用于为低带宽压缩格式(compressed formats)image传输提供透明支持
    image_transport::ImageTransport it(n);
    image_transport::Publisher pub_image_left = it.advertise("image_left", 2);
    image_transport::Publisher pub_image_right = it.advertise("image_right", 2);
    nav_msgs::Odometry odomGT;
    // 真值pose的坐标系实际是左相机的坐标系
    odomGT.header.frame_id = "camera_init";
    odomGT.child_frame_id = "ground_truth";
    nav_msgs::Path pathGT;
    pathGT.header.frame_id = "camera_init";
    // 读取时间戳文件
    std::string timestamp_path = "sequences/" + sequence_number + "/times.txt";
    // 读取ground_truth_path
    std::string ground_truth_path = "results/" + sequence_number + ".txt";
    std::ifstream timestamp_file(dataset_folder + timestamp_path, std::ifstream::in);
    std::ifstream ground_truth_file(dataset_folder + ground_truth_path, std::ifstream::in);
    // 函数原型，void rosbag::Bag::open(const std::__cxx11::string &filename, uint32_t mode = 2U)
    rosbag::Bag bag_out;
    if (to_bag) bag_out.open(output_bag_file, rosbag::bagmode::Write);
    // camera 相对于lidar的旋转矩阵 其实更准确的应该使用KITTI提供的左相机到Lidar的标定参数进行变换
    Eigen::Matrix3d R_transform;
    R_transform << 0, 0, 1, -1, 0, 0, 0, -1, 0;
    Eigen::Quaterniond q_transform(R_transform);
    std::string line;                   // 声明字符串line
    std::size_t line_num = 0;           // 声明读取的行数
    // 发布频率
    ros::Rate r(10.0 / publish_delay);
    // 读 时间戳 文件 成为一个字符串
    while (std::getline(timestamp_file, line) && ros::ok())
    {
        // stof()将字符串转换为float型
        float timestamp = stof(line);
        std::stringstream left_image_path, right_image_path, lidar_data_path;
        // 自动填充'0'使得'0'+line_num凑成6个字符
        left_image_path << dataset_folder << "sequences/" + sequence_number + "/image_0/" << std::setfill('0')
                        << std::setw(6) << line_num << ".png";
        right_image_path << dataset_folder << "sequences/" + sequence_number + "/image_1/" << std::setfill('0')
                         << std::setw(6) << line_num << ".png";
        lidar_data_path << dataset_folder << "velodyne/sequences/" + sequence_number + "/velodyne/"
                        << std::setfill('0') << std::setw(6) << line_num << ".bin";
        cv::Mat left_image = cv::imread(left_image_path.str(), cv::IMREAD_GRAYSCALE);
        cv::Mat right_image = cv::imread(left_image_path.str(), cv::IMREAD_GRAYSCALE);
        // 得到ground truth的文件，这里没用
        std::getline(ground_truth_file, line);
        // 读取 ground_truth的一行,一行12个数，塞到矩阵 gt_pose 里
        std::stringstream pose_stream(line);
        std::string s;
        Eigen::Matrix<double, 3, 4> gt_pose;
        for (std::size_t i = 0; i < 3; ++i)
        {
            for (std::size_t j = 0; j < 4; ++j)
            {
                std::getline(pose_stream, s, ' ');
                gt_pose(i, j) = stof(s);
            }
        }

        Eigen::Quaterniond q_w_i(gt_pose.topLeftCorner<3, 3>());            // 提取位姿矩阵的旋转矩阵部分 并转成四元数
        //Eigen::Quaterniond q = q_transform * q_w_i;                       // 此处有bug，修改成下一行
        Eigen::Quaterniond q = q_transform * q_w_i * q_transform.inverse(); // 坐标系进行变换的四元数 q
        q.normalize();                                                      // 对四元数 q 归一化
        // q 来自于 q_w_i, q_w_i来自于 gt_pose， gt_pose来自于ground_truth_file，也就是真值文件.txt
        // t 来自于 gt_pose， q_transform是四元数，和3*1向量相乘，有运算法则
        Eigen::Vector3d t = q_transform * gt_pose.topRightCorner<3, 1>();
        // odomGT 又来自于 q，t
        odomGT.header.stamp = ros::Time().fromSec(timestamp);
        odomGT.pose.pose.orientation.x = q.x();
        odomGT.pose.pose.orientation.y = q.y();
        odomGT.pose.pose.orientation.z = q.z();
        odomGT.pose.pose.orientation.w = q.w();
        odomGT.pose.pose.position.x = t(0);
        odomGT.pose.pose.position.y = t(1);
        odomGT.pose.pose.position.z = t(2);
        pubOdomGT.publish(odomGT);
        // 又把odomGT 放到了 poseGT
        geometry_msgs::PoseStamped poseGT;
        poseGT.header = odomGT.header;
        poseGT.pose = odomGT.pose.pose;
        // nav_msgs::Path pathGT; 用于ROS中画轨迹
        // 又把odomGT 放到了 pathGT
        // nav_msgs::Path = std_msgs/Header + geometry_msgs::PoseStamped
        pathGT.header.stamp = odomGT.header.stamp;
        pathGT.poses.push_back(poseGT);
        pubPathGT.publish(pathGT);

        // 读取lidar的数据，以[x,y,z,i]存储在向量中
        std::vector<float> lidar_data = read_lidar_data(lidar_data_path.str());
        // 以四个元素（xyzi）为一个点，总共打印lidar_data.size() / 4.0个点
        std::cout << "totally " << lidar_data.size() / 4.0 << " points in this lidar frame \n";
        std::vector<Eigen::Vector3d> lidar_points;   // 声明雷达点的位置向量
        std::vector<float> lidar_intensities;        // 声明雷达点的强度向量
        pcl::PointCloud<pcl::PointXYZI> laser_cloud; // 声明一个pcl的雷达点云
        for (std::size_t i = 0; i < lidar_data.size(); i += 4)
        {
            // emplace_back是就地构造，不用构造后再次复制到容器中。因此效率更高
            lidar_points.emplace_back(lidar_data[i], lidar_data[i+1], lidar_data[i+2]);  // 位置XYZ塞到3维向量lidar_points
            // 点云强度塞到浮点一维向量lidar_intensities
            lidar_intensities.push_back(lidar_data[i+3]);
            pcl::PointXYZI point;
            point.x = lidar_data[i];
            point.y = lidar_data[i + 1];
            point.z = lidar_data[i + 2];
            point.intensity = lidar_data[i + 3];
            laser_cloud.push_back(point);
        }

        sensor_msgs::PointCloud2 laser_cloud_msg;
        pcl::toROSMsg(laser_cloud, laser_cloud_msg);
        laser_cloud_msg.header.stamp = ros::Time().fromSec(timestamp);
        laser_cloud_msg.header.frame_id = "camera_init";
        pub_laser_cloud.publish(laser_cloud_msg);
        // 将opencv的图像转换成ros的图像
        sensor_msgs::ImagePtr image_left_msg = cv_bridge::CvImage(
                laser_cloud_msg.header, "mono8", left_image).toImageMsg();
        sensor_msgs::ImagePtr image_right_msg = cv_bridge::CvImage(
                laser_cloud_msg.header, "mono8", right_image).toImageMsg();
        pub_image_left.publish(image_left_msg);
        pub_image_right.publish(image_right_msg);
        if (to_bag)
        {
            bag_out.write("/image_left", ros::Time::now(), image_left_msg);
            bag_out.write("/image_right", ros::Time::now(), image_right_msg);
            bag_out.write("/velodyne_points", ros::Time::now(), laser_cloud_msg);
            bag_out.write("/path_gt", ros::Time::now(), pathGT);
            bag_out.write("/odometry_gt", ros::Time::now(), odomGT);
        }
        line_num++;
        r.sleep();
    }
    // 关闭输出的rosbag
    bag_out.close();
    std::cout << "Done \n";
    return 0;
}
